#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <geometry_msgs/Twist.h>
#include <iostream>
#include <cstdlib>
#include <ctime>
#include <actionlib/server/simple_action_server.h>
#include <turtle_multipoints/turtleAction.h> //编译turtle.action后自动生成的文件，在lyh_workspace/devel/include/turtle_multipoints中

void check_theta(float& fTheta)//定义一个全局函数，使Theta在-pi到pi之间
{
    if(fTheta<-3.1415){
        fTheta+=6.283;
    }
    else if(fTheta>3.1415){
        fTheta-=6.283;
    }
}

class Turtle_class//声明一个类
{
    public://构造函数，及回调函数
        Turtle_class(std::string mame);
        void turtle_move(const turtle_multipoints::turtleGoalConstPtr& goal);
        void posecallback(const turtlesim::PoseConstPtr& msg);
    private:
        ros::NodeHandle nh_;
        actionlib::SimpleActionServer<turtle_multipoints::turtleAction> as_;//声明一个action服务对象
        std::string action_name_;
        std::vector<float> goal_x;
        std::vector<float> goal_y;
        turtle_multipoints::turtleFeedback feedback_;//声明feedback成员
        turtle_multipoints::turtleResult result_;//声明result成员
        ros::Subscriber turtle_pose_sub;//声明订阅
        ros::Publisher turtle_vel_pub;//声明发布
        double vlinear, vangular;//变量
        float turtle_present_pose_x, turtle_present_pose_y, turtle_present_pose_theta,
        turtle_target_pose_x, turtle_target_pose_y,
        error_x, error_y, error_theta, error_linear;
};

Turtle_class::Turtle_class(std::string name):as_(nh_,name,boost::bind(&Turtle_class::turtle_move,this,_1),false),action_name_(name)//将参数绑定在函数上，当函数得到name的时候，赋值给
{
    /*
    将参数绑定在函数上，当函数得到值name的时候，赋值给其他参数
        :as_(nh_,name,boost::bind(&Turtle_class::turtle_move,this,_1),false)
        声明一个SimpleActionServer。
            param1: 'nh_'为节点句柄
            param2: 'name'为动作服务器的名称，这个名称会成为其所有子话题的命名空间，TimerAction为动作服务器的类型，
            param3: 'boost::bind(&Turtle_class::turtle_move,this,_1)' 
                为目标的回调函数，等价于&Turtle_class::(this, _1)
                param1: 'this'是 C++ 中的一个关键字，也是一个 const 指针，它指向当前对象，通过它可以访问当前对象的所有成员。
                param2: '_1' 如果函数对象有内部类型定义result_type，那么bind可以自动推导出返回值类型
                                例：bind(std::greater<int>(),_1,10);  //检查 x>10
            param4: 'false' 为目标的回调函数，False 参数表示关闭动作服务器的自动启动功能。
    */
    unsigned seed; //定义一个无符号变量
    seed = std::time(0);
    std::srand(seed);
    /*
    函数名:   srand   
    功   能:   初始化随机数发生器   
    用   法:   void srand(unsigned  int  seed);  
    所在头文件： stdlib.h
    函数说明：
               srand()用来设置rand()产生随机数时的随机数种子。
               参数seed必须是个整数，通常可以利用time(0)的返回值或NULL来当做seed。
               如果每次seed都设相同值，rand()所产生的随机数值每次就会一样。
    std::rand()
    函数名:   rand   
    功   能:   随机数发生器 
    用   法:   int rand(void);  
    所在头文件: stdlib.h  
    */
    turtle_vel_pub = nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1);//发布速度信息到"turtle1/cmd_vel"消息队列的最大长度为1。
    turtle_pose_sub = nh_.subscribe<turtlesim::Pose>("/turtle1/pose", 10, &Turtle_class::posecallback, this);//订阅乌龟的位置信息
    as_.start();//actionserver开始
}

void Turtle_class::posecallback(const turtlesim::PoseConstPtr& msg)//订阅回调函数
{
    //获取乌龟的pose位置
    turtle_present_pose_x = msg->x;
    turtle_present_pose_y = msg->y;
    turtle_present_pose_theta = msg->theta;
    check_theta(turtle_present_pose_theta);//转换角度在3.1415到-3.1415之间 
}

void Turtle_class::turtle_move(const turtle_multipoints::turtleGoalConstPtr &goal)//actionserver的目标点函数
{
    bool success = true;
    geometry_msgs::Twist turtle_vel_msgs;//声明一个速度格式的对象
    goal_x.clear(); //清空goal_x的所有元素
    goal_y.clear();

    /*生成导航点*/
    for(int i=0;i<goal->point_num;i++){//生成要求个数的随机点
        float x,y;
        x = (float)(rand()%10)+(float)(rand()%100)/100.0;//%为求余,rand()%为0~9之间的数，rand()%100为0~99，0.00<x<9.99
        y = (float)(rand()%10)+(float)(rand()%100)/100.0;
        goal_x.push_back(x);//函数将一个新的元素加到vector的最后面，位置为当前最后一个元素的下一个元素
        goal_y.push_back(y);
        /*    
        类似的：
            pop_back() 移除最后一个元素
            clear() 清空所有元素
            empty() 判断vector是否为空，如果返回true为空
            erase()删除指定元素
        */
    }

    /*开始巡航*/
    for(int i=0;i<goal->loop_time;i++)//重复巡航
    {
        for(int j=0;j<goal->point_num;j++){//依次控制小乌龟到达各个目标点
            turtle_target_pose_x = goal_x[j];
            turtle_target_pose_y = goal_y[j];
            std::cout<<"x:"<<turtle_target_pose_x<<std::endl;
            std::cout<<"y:"<<turtle_target_pose_y<<std::endl;

            if(as_.isPreemptRequested()||!ros::ok())//preempt是否取消正在执行的动作
            {
                ROS_INFO("%s:Preempted", action_name_.c_str());
                as_.setPreempted();
                success = false;
                goto flag; //无条件转移语句,跳出循环
            }

            while(ros::ok()){
                error_x = turtle_target_pose_x-turtle_present_pose_x;//x方向差值
                error_y = turtle_target_pose_y-turtle_present_pose_y;//y方向差值
                error_linear = sqrt(pow(error_x,2)+pow(error_y,2));//目标位置和当前为止的直线差值
                turtle_vel_msgs.linear.x=2.0*error_linear;//计算x方向的速度
                if((error_y<0.001)&&(error_y>-0.001)&&(error_x<0.0)){
                    turtle_vel_msgs.angular.z = 20.0*(-3.1415-turtle_present_pose_theta);//计算角速度
                }
                else
                {
                    error_theta = atan2(error_y,error_x)-turtle_present_pose_theta;//计算角度偏差
                    check_theta(error_theta);
                    turtle_vel_msgs.angular.z = 5.0*error_theta;//计算角速度
                }
                turtle_vel_pub.publish(turtle_vel_msgs);
                if(fabs(error_linear)<0.01) break;
                usleep(10000*10);
                ros::spinOnce();
            }
            feedback_.feedback_x=turtle_present_pose_x;
            feedback_.feedback_y=turtle_present_pose_y;
            feedback_.current_loop = i+1;
            as_.publishFeedback(feedback_);//发布当前的位置状态
        }
    }


    if(success)//状态标志判读，当动作正确完成后，发布结果，使客户端得到结果
    {
        result_.goal_num = goal->point_num;
        ROS_INFO("%s:Succeeded", action_name_.c_str());
        as_.setSucceeded(result_);//发布
    }
flag:;//goto强制跳转到此
}

int main(int argc, char** argv)//主函数入口
{
    ros::init(argc,argv,"turtleAct_server");//初始化
    Turtle_class turtleAction("turtleAction");//TurtleAction声明一个类对象
    ros::spin();//使程序和后台数据进入回调函数
    return 0;
}
